command igus-dryve with canopen_motor_node
Hy there,
i am trying to implement a controller for a igus-dryve motor drive on ros-kinetic. I am using canopen and the ros_canopen package to communicate over a sysWORXX USBCanmodule 1 with the igus-dryve and have everything setup properly (stepper motor in closed loop beeing controllable over the igus webinterface and set to accept CANopen communication which is wired correctly).
Calling rosservice call /joint/driver/init
works fine, axis gets reset, return is True and the ros-controllers are loading.
Here comes my issue:
- How do I actually move/command the motor? By setting the actual values of the Object by
rosservice call /joint/driver/setObject
? Or does the canopen_motor_node take care of all the communication in order to drive the motor? In that case how do I forward a positioning command? I tried to callrostopic pub /lifter_ros/lifter_joint_position_controller/command std_msgs/Float64 "data: 1.0"
but no reactions. - Additionally i was thinking about if ros_control is really needed in my use-case? igus-dryve already performs a closed-loop control so isn`t ros_control a layer ontop and too much?
Please find attached my controller config:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
lifter_joint_position_controller:
type: position_controllers/JointPositionController
joint: lifter_joint
required_drive_mode: 1
And my node layer:
name: igus-dryve
defaults:
eds_pkg: lifter_ros
eds_file: "config/igus_dryve_D1.eds" #electrical desciption file of igus dryve
#the following scaling is due to 16bit value range limitation of velocity command in vl mode (2)
#vel_to_device: "rint(rad2deg(vel)*250)"
#dcf_overlay: # "ObjectID": "ParameterValue" (both as strings)
# "604Csub1": "1" # vl dimension factor numerator
# "604Csub2": "24000" # vl dimension factor denominator
#defaults: # optional, all defaults can be overwritten per node
# canopen_chain_node settings ..
motor_allocator: canopen::Motor402::Allocator # select allocator for motor layer
#motor_layer: #settings passed to motor layer (plugin-specific)
switching_state: 5 # (Operation_Enable), state for mode switching
pos_to_device: "rint(rad2deg(pos)*1000)" # rad -> mdeg
pos_from_device: "deg2rad(obj6064)/1000" # actual position [mdeg] -> rad
vel_to_device: "rint(rad2deg(vel)*1000)" # rad/s -> mdeg/s
vel_from_device: "deg2rad(obj606C)/1000" # actual velocity [mdeg/s] -> rad/s
eff_to_device: "rint(eff)" # just round to integer
eff_from_device: "0" # unset
nodes:
lifter_joint:
id: 2
and my launchfile:
?xml version="1.0"?>
<launch>
<!-- send urdf to param server -->
<param name="robot_description" command="cat '$(find lifter_ros)/urdf/lifter.urdf'" />
<!-- robot state publisher -->
<node ns="lifter_ros" pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
<!--remap from="joint_states" to="/arm/joint_states"/-->
</node>
<node ns="lifter_ros" name="driver" pkg="canopen_motor_node" type="canopen_motor_node" output="screen" clear_params="true" launch-prefix="">
<rosparam command="load" file="$(find lifter_ros)/config/CANopen_layer.yaml" />
<rosparam command="load" file="$(find lifter_ros)/config/node_layer.yaml" />
</node>
<!-- controllers -->
<rosparam ns="lifter_ros" command="load" file="$(find lifter_ros)/config/dryve_controller.yaml" />
<!-- start_controllers -->
<node ns="lifter_ros" name="lifter_ros_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>
</launch>
I would highly appreciate your help!
Thanks
EDIT Regarding the controllers: The output says the controller ...